using System;
using System.Collections.Generic;
using System.Text;

using XNAF = Microsoft.Xna.Framework;

using XGAE.Framework;
using xExt = XGAE.Framework.Extensions;
using xBackend = XGAE.Framework.Backend;
using xBody = XGAE.Framework.Body;

namespace XGAE.Body.Physics
{
    class PhysicsModule : Component, IPhysicsBase
    {
        #region Fields

        private XNAF.Vector3 _gPosition; // global coordinate of the animat in 3D

        public XNAF.Vector3 GPosition
        {
            get
            {
                return this._gPosition;
            }
        }

        private xExt.Angles _gOrientation; // euler angles representing the position

        internal xExt.Angles GOrientation
        {
            get
            {
                return this._gOrientation;
            }
        }

        private XNAF.Vector3 _rPosition; // accumulation of the relative rotation

        public XNAF.Vector3 RPosition
        {
            get
            {
                return this._rPosition;
            }
        }

        private xExt.Angles _rOrientation; // accumulation of the relative rotation

        public xExt.Angles ROrientation
        {
            get
            {
                return this._rOrientation;
            }
        }

        private XNAF.Matrix _mDirection; // orientation matrix of the animat in space

        public XNAF.Matrix MDirection
        {
            get
            {
                return this._mDirection;
            }
        }

        private XNAF.Matrix _mInverse; // inverse of the orientation for conversions

        public XNAF.Matrix MInverse
        {
            get
            {
                return this._mInverse;
            }
        }

        #region IPhysicsBase Implementation

        protected xBackend.IPhysics _backend; // backend hook

        public xBackend.IPhysics Backend
        {
            get { return this._backend; }
        }

        protected List<Interface> _stateChannel; // collects all exported State interfaces

        public List<Interface> StateChannel
        {
            get { return this._stateChannel; }
        }

        #endregion

        #endregion

        #region C'tors

        protected PhysicsModule()
            : base("Physics")
        { }

        #endregion

        #region Private Methods

        /// <summary>
        /// Odometric query to check how far we have gone relatively to the last position.
        /// </summary>
        internal void GetMovement(out XNAF.Vector3 movement)
        {
            // return the relative offset divided by step size 
            movement = XGAE.Helpers.Multiply(this._mInverse, this._rPosition);
        }

        /// <summary>
        /// Assertain how we've rotated from the previous position.
	    /// </summary>
        internal xExt.Angles GetOrientation()
        {
            // clamp the values of the angles in degrees to prevent overflow
            for (int i = 0; i < 3; ++i)
            {
                if (this._rOrientation[i] < -180.0f) this._rOrientation[i] += 360.0f;
                if (this._rOrientation[i] > 180.0f) this._rOrientation[i] -= 360.0f;
            }
            // return the accumulation of the relative angular rotation
            return this._rOrientation;
        }

        #endregion

        #region Common Interface (Component)

        /*/// <summary>
        /// Main entry point for the AI called regularly by the framework.
        /// </summary>
	    public override void Think()
        {
        }*/

	    /// <summary>
        /// Custom initialisation of the data once everything has been loaded.
	    /// </summary>
	    public override void Init()
        {
            // reset the global position so the starting point is the center of the world
            _backend.GetLocation(_gPosition, _gOrientation);
            // clear the accumulators now for accurate tracking
            _rPosition = XNAF.Vector3.Zero;
            _rOrientation.Clear();

            // compute the corresponding rotation matrix
            _mDirection = XNAF.Matrix.CreateFromYawPitchRoll(_gOrientation.Yaw, _gOrientation.Pitch, _gOrientation.Roll);
            _mInverse = _mDirection;
            _mInverse = XNAF.Matrix.Invert(_mInverse);
        }

	    /// <summary>
        /// Prepare the data before everything is saved automatically.
	    /// </summary>
	    public override void Done()
        {
        }

        #endregion

        #region IPhysicBase Implementation

        /// <summary>
        /// Scan through all the interfaces in this channel, and dispatch the message to each one. 
        /// </summary>
        /// <param name="m"></param>
        public void BroadcastState(Message m)
        {
            foreach (Interface i in this._stateChannel)
            {
                i.Dispatch(m);
            }
        }

        #endregion

        #region Message Handlers

        /// <summary>
        /// Callback for Collision messages using expanded parameters.
        /// Asynchronous interuption of control when a collision occurs with an obstacle.
        /// </summary>
        /// <param name="normal"></param>
        protected void OnCollision(XNAF.Vector3 normal)
        {
            BroadcastState(new xBody.Physics.CollisionMessage(XGAE.Helpers.Multiply(this._mInverse , normal)));
        }

        /// <summary>
        /// Get the global location of the entity in the game world.
        /// Callback for Update messages using expanded parameters.
        /// </summary>
        /// <param name="position"></param>
        /// <param name="orientation"></param>
        protected void OnUpdate(XNAF.Vector3 position, xExt.Angles orientation)
        {
            // FIXME: when do we clear the accumulators?
            // reset the accumulator so the tracking is accurate 
            _rOrientation.Clear();
            // reset the accumulator so the dead reckoning is accurate 
            _rPosition = XNAF.Vector3.Zero;

            // accumulate the angular rotation for later
            _rOrientation.Incriment(xExt.Angles.Minus(orientation, _gOrientation));
            // store the new global orientation
            _gOrientation = orientation;

            // keep track of the position change in the same way
            _rPosition += position - _gPosition;
            // store the new global position
            _gPosition = position;

            // compute the rotation matrix and its inverse
            XNAF.Matrix.CreateFromYawPitchRoll(orientation.Yaw, orientation.Pitch, orientation.Roll, out _mDirection);
            _mInverse = _mDirection;
            XNAF.Matrix.Invert(ref _mInverse,out  _mInverse);
        }

        #endregion
    }
}
